import sensor,image,time,car,esp32,pyb,math,text,pid

#初始化摄像头
sensor.reset()  #重启摄像头
sensor.set_pixformat(sensor.RGB565) #设置彩色
sensor.set_framesize(sensor.QQVGA)  #设置画面分辨率。因为内存限制，识别AprilTags不能开更高分辨率。
sensor.set_vflip(True)     #针对小车硬件，翻转画面方向
sensor.set_hmirror(True)
sensor.skip_frames(time = 2000) #等待2秒，摄像头画面稳定
sensor.set_auto_gain(False)     #关闭自动亮度
sensor.set_auto_whitebal(False) #关闭自动白平衡
#声明WiFi图传模块（自动连接WiFi）
wifi=esp32.wifi_video_stream()  #声明ESP32 WiFi图传模块。这是UP自制的外设和库。
wifi_img=sensor.alloc_extra_fb(160,160,sensor.RGB565)   #声明用于wifi图传的画布。
wifi_img.draw_rectangle(0,0,160,40,fill=True,color=(30,30,30)) #画布上侧绘制显示数据区底色
wifi_img.draw_rectangle(0,0,160,40,color=(255,255,255))   #绘制数据区边框边框
#蜂鸣器响起预备音
buzzer=car.buzzer() #声明蜂鸣器。这是UP自制的外设和库。
for n in range(3):
    buzzer.frequency(440)
    time.sleep(0.5)
    buzzer.frequency(0)
    time.sleep(0.5)
buzzer.frequency(600)
time.sleep(1)
buzzer.frequency(0)
#在画布上，绘制球的状态（抓到多少只球）
get_balls=[0,0,0]   #记录抓到多少只球。三个元素分别代表红球、绿球、篮球
def draw_balls():
    global wifi_img,get_balls   #声明全部变量
    for n in range(2):  #画两个红色圆圈
        if n<get_balls[0]:#如果已经抓到红球，则画实心圆
            wifi_img.draw_circle(133+n*13,10,3,fill=True,color=(255,0,0))
        else:   #如果没有抓到红球，画空心圆
            wifi_img.draw_circle(133+n*13,10,3,color=(255,0,0))
    for n in range(2):  #同上
        if n<get_balls[1]:
            wifi_img.draw_circle(133+n*13,20,3,fill=True,color=(0,255,0))
        else:
            wifi_img.draw_circle(133+n*13,20,3,color=(0,255,0))
    for n in range(2):  #同上
        if n<get_balls[2]:
            wifi_img.draw_circle(133+n*13,30,3,fill=True,color=(0,0,255))
        else:
            wifi_img.draw_circle(133+n*13,30,3,color=(0,0,255))
draw_balls()#开机首次绘制圆圈。

clock = time.clock()    #声明用于记录帧速的时钟
timer=pyb.millis()  #显示时间所用的，记录时间变量
timer_wifi_stream=pyb.millis()  #控制WiFi图传刷新路所用的，记录时间变量
fps=0   #显示帧速所用的变量
f_x = 180   #AprilTag尺寸比例系数,用于换算真实世界中的尺寸数值。与镜头焦距、镜头像素、AprilTag尺寸有关
f_y = 180   #实测5cm的ArpilTage在此平台上的系数是180。
            #对于其他尺寸的ArpilTage，测定比例数值公式：f_x=f_y=180/tag.z_translation()*距离*-1 。距离单位cm
c_x = 160 * 0.5 #设置画面中心点，像素数/2
c_y = 120 * 0.5
self_x=0    #记录小车自己在场地中的位置，单位cm
self_y=0
self_direction=0    #记录小车自己在场地中的角度，以弧度制顺指针旋转
#声明补光灯，并打开
led=car.led()
led.turn_on()
#声明驱动，UP自制的外设和库。
drive_close=car.drive_close_loop()  #声明闭环驱动。
#后续程序中只有自转180度使用了闭环驱动，所以闭环驱动必要性不大。
drive_close.lock_degrees(False) #关闭闭环驱动锁止角度（不关闭会与开环驱动冲突）
drive_open=car.drive()  #声明开环驱动
#声明pid。PID库是OpenMV自带的。关注UP，后续会讲解PID。
pid_lr=pid.PID(p=1,i=0.1,imax=30)   #抓球时对准球的平移所用的PID
pid_fb=pid.PID(p=0.015,i=0.06,d=0.001,imax=30)#抓球时对准球的平移所用的PID
#AprilTags定位用的PID
pid_tags_lr=pid.PID(p=1.3,i=0.5,d=0,imax=30)    #平移
pid_tags_fb=pid.PID(p=4,i=4,d=1,imax=30)    #前后
pid_tags_r=pid.PID(p=1.3,i=0.6,d=0,imax=30) #自转
#控制机械臂函数
servo=car.servo_motor() #声明舵机控制。这是UP自制的外设和库。
def arm_control(mode):
    if mode==0:             #找球状态
        servo.degree(0,120)
        servo.degree(1,43)
        servo.degree(2,49)
    elif mode==1:           #预备抓球
        servo.degree(0,55)
        servo.degree(1,143)
        servo.degree(2,150)
    elif mode==2:           #合爪子抓球
        servo.degree(0,24)
    elif mode==3:           #摄像头看球确认
        servo.degree(1,88)
        servo.degree(2,58)
    elif mode==4:           #持球
        servo.degree(0,24)
        servo.degree(1,43)
        servo.degree(2,49)
    elif mode==5:           #预备放球
        servo.degree(0,24)
        servo.degree(1,60)
        servo.degree(2,100)
    elif mode==6:           #放球
        servo.degree(0,35)
        servo.degree(1,60)
        servo.degree(2,100)
    elif mode==7:           #庆祝完成
        drive_open.move(0,0,30)
        servo.degree(1,55)
        servo.degree(2,125)
        while True:
            servo.degree(0,18)
            time.sleep(0.3)
            servo.degree(0,43)
            time.sleep(0.3)
arm_control(0)  #开机控制机械臂到初始状态

#红球阈值#绿球阈值#蓝球阈值
(17, 68, 30, 81, -10, 50)
thresholds=([17,68,30,81,-10,50],[33,92,-68,-13,15,55],[15,75,-20,45,-88,-30])
#[15, 75, -15, 45, -88, -30]
target_pixels=1700  #适合抓取的球像素（像素和距离成正比）
#找球之后绘制选择框的颜色
target_rectangle_colors=((255,0,0),(0,255,0),(0,0,255))
#抓球部分变量
aim_times=0 #瞄准计次，达到次数则认定瞄准成功
mode=0 #模式，0:找球，1：追球、抓球、检查是否持球，2：送球到筐
step=0 #抓球等步骤
step_timer_target=0 #步骤用的计时的目标
step_timer_count=0  #步骤用的计时的计时器
hold_ball_check_timer=0 #持球检查计时，
max_ball_index=1 #最大的球的序号，0红，1绿，2蓝
get_balls=[0,0,0]#获取到的球的数量，元素分别是红球、绿球、篮球

#墙上的AprilTags信标坐标，由x坐标，y坐标，角度组成。单位cm、弧度。
beacon_tags=((0,20,4.71),(0,40,4.71),(0,60,4.71),(0,80,4.71),(0,100,4.71),\
(20,120,0),(40,120,0),(60,120,0),(80,120,0),(100,120,0),\
(120,100,1.57),(120,80,1.57),(120,60,1.57),(120,40,1.57),(120,20,1.57),\
(100,0,3.14),(80,0,3.14),(60,0,3.14),(40,0,3.14),(20,0,3.14),)
#抓球部分子函数。抓球是mode1。本来写在主循环中就行，开始结构没有规划好，单立起为子函数了。
def ball_catch(lr,fb):#参数分别代表平移量，前后量
    global aim_times,target_pixels,mode,hold_ball_check_timer,step
    global step_timer_count,step_timer_target   #声明全局变量。
    if aim_times<7: #没达到瞄准次数
        if target_pixels-150<fb<target_pixels+150 and 75<lr<85: #符合位置参数
            aim_times+=1    #瞄准变量自加
        else:   #不符合位置参数
            aim_times=0 #瞄准变量归零
        #用开环移动，通过PID调节输出移动所需的参数。
        #move函数的参数为（平移量，前后量，自转量）
        drive_open.move(pid_lr.get_pid(lr-80,1),pid_fb.get_pid(target_pixels-fb,1),0)
        step=0  #进入步骤0
    else:   #达到了瞄准次数
#步骤0，预备抓球状态
        if step==0:
            drive_open.move(0,0,0)  #停止移动
            arm_control(1)  #预备抓球
            step_timer_target=800   #下一步之前延迟800ms
            step_timer_count=pyb.millis()   #重置计时变量
            step=1  #进入步骤1
#步骤1，前进
        elif step==1:
            if pyb.millis()-step_timer_count>step_timer_target:#达到计时值
                drive_open.move(0,22,0) #前进
                step_timer_target=220   #下一步之前延迟200ms
                step_timer_count=pyb.millis()   #重置计时
                step=2  #进入步骤2
#步骤2，合爪子
        elif step==2:
            if pyb.millis()-step_timer_count>step_timer_target:#达到计时值
                arm_control(2)  #合爪子
                step_timer_target=300   #下一步之前延迟300ms
                step_timer_count=pyb.millis()   #重置计时
                step=3  #进入步骤3
#步骤3，抬爪子
        elif step==3:
            if pyb.millis()-step_timer_count>step_timer_target:#达到计时值
                arm_control(3)  #抬爪子进入确认状态
                step_timer_target=100   #下一步之前延迟100ms
                step_timer_count=pyb.millis()   #重置计时
                hold_ball_check_timer=pyb.millis()#重置持球检测计时
                step=4  #进入步骤4
#步骤4，确认爪子内是否有球
        elif step==4:
            if pyb.millis()-step_timer_count>step_timer_target:#达到计时值
                drive_open.move(0,0,0)  #停止移动
                pid_lr.reset_I()    #重置移动的I
                pid_fb.reset_I()
                step_timer_target=400   #下一步之前延迟400ms
                step_timer_count=pyb.millis()   #重置计时
                if pyb.millis()-hold_ball_check_timer<1000: #一秒内检测是否有球
                    if fb>3000: #检查持球的判断阈值，像素大于3000
                        arm_control(3)  #抬爪子进入确认是否持球状态
                        step_timer_target=800   #下一步之前延迟200ms
                        step_timer_count=pyb.millis()   #重置计时
                        step=5  #进入步骤5
                else:#检测是否有球超时
                    arm_control(0)  #爪子进入找球模式
                    step_timer_target=400   #下一步之前延迟400ms
                    step_timer_count=pyb.millis()   #重置计时
                    step=6  #进入步骤6
#步骤5，持球成功
        elif step==5:
            if pyb.millis()-step_timer_count>step_timer_target:#达到计时值
                arm_control(4)  #爪子进入持球状态
                mode=2  #进入mode2
                step=0  #step归零
#步骤6，持球失败
        elif step==6:
            if pyb.millis()-step_timer_count>step_timer_target:#达到计时值
                mode=0  #返回mode0重新开始
                step=0  #step归零

#记录球的位置，分别为红、绿、蓝色。每种颜色中分别为cx值（横向坐标），pixels值（像素，和前后距离成正比）
balls_location=[[-1,-1],[-1,-1],[-1,-1]]
motor_mileage=[0,0,0,0] #记录电机运转角度。逆解位置暂时未做...
tags=[-1] #AprilTags识别结果，-1是没有找到AprilTags
#抓到球、看到AprilTags之后，去筐的线路。列表一级索引为当前所在边，列表二级索引为颜色。
#内容数字为需要向哪个方向直线移动的次数。比如-2，为向左直行，左转，向左直行
route_to_basket=((1,2,-2),(-1,1,2),(-2,-1,1),(2,-2,-1))
to_corner_tag_id=-1
first_time_to_corner=True   #首次进入角落。配合上面线路规划，首次进入角落才计算转弯次数，避免重复
#主循环
while True:
    clock.tick()#用于显示帧的函数，需要在循环开头执行
    img=sensor.snapshot()   #获取摄像头画面
#看AprilTags获取位置，在模式2才执行。其他找球抓球等的模式下，不执行，以提高帧速
    if mode==2:
        tags=img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y)#在画面中找AprilTags。
        if tags:#如果有结果
            tags_max = max(tags, key = lambda b: b.z_translation())#找距离最近的Tag
            img.draw_rectangle(tags_max.rect(), color = (255, 0, 0))    #在Tag上画方框
            img.draw_cross(tags_max.cx(), tags_max.cy(), color = (0, 255, 0))#画十字
            if tags_max.id()<20:    #如果Tag序号小于20，则是墙壁的定位码
                #通过定位码的坐标库，和识别到的码与自己的相对距离、角度。计算自己在场地中的位置。
                self_x=beacon_tags[tags_max.id()][0]+math.sin(tags_max.y_rotation()+beacon_tags[tags_max.id()][2])*tags_max.z_translation()
                self_y=beacon_tags[tags_max.id()][1]+math.cos(tags_max.y_rotation()+beacon_tags[tags_max.id()][2])*tags_max.z_translation()
                self_direction=beacon_tags[tags_max.id()][2]+tags_max.y_rotation()
                #对应四面墙壁上的码，增减横向距离
                if 0<=tags_max.id()<5:
                    self_y+=tags_max.x_translation()*-2
                elif 5<=tags_max.id()<10:
                    self_x+=tags_max.x_translation()*-2
                elif 10<=tags_max.id()<15:
                    self_y+=tags_max.x_translation()*2
                elif 15<=tags_max.id()<20:
                    self_x+=tags_max.x_translation()*2
        else:   #没有找到AprilTags
            tags=[-1]   #列表置-1
#分别识别三色
    for n in range(3):  #循环三次
        #通过三色的阈值，依次识别。阈值可以用OpenMV的阈值编辑器获取
        blobs = img.find_blobs([thresholds[n]], x_stride=2, y_stride=2)
        if blobs:#如果找到结果
            blobs_max = max(blobs, key = lambda b: b.pixels())#按结果的像素值，找最大值的数据。也就是找最大的色块。
            if blobs_max.w()>5 and blobs_max.h()>5:#过滤掉长宽小于5的结果
                balls_location[n]=[blobs_max.cx(),blobs_max.pixels()]#将色块对应的横向值，像素（前后值）装入变量。
                img.draw_rectangle(blobs_max[0:4],color=target_rectangle_colors[n])#按寻找色块结果的前四个值，绘制方形，框选识别结果。
                img.draw_cross(blobs_max.cx(),blobs_max.cy(),color=target_rectangle_colors[n])#用结果的中心值坐标，绘制十字
            else:   #没有结果
                balls_location[n]=[-1,-1]   #装填-1
        else:   #没有结果
            balls_location[n]=[-1,-1]   #装填-1
#整体步骤（模式）：0-找球，1-追球、抓球、检查球，2-移动到框、投放，3-结束庆祝
#模式0，找球
    if mode==0:#模式0，找球
        led.turn_on()   #打开补光灯
        drive_open.move(-30,0,0)    #向左平移
        #在识别到的三种颜色的球中，找像素最大的（距离最近的），获取其序号（序号代表颜色0红1绿2蓝）
        max_ball_index=balls_location.index(max(balls_location,key = lambda b: b[1]))
        #通过PID输出控制开环移动库平移到，对准目标球
        drive_open.move(pid_lr.get_pid(balls_location[max_ball_index][0]-80,1),0,0)
        if 70<balls_location[max_ball_index][0]<90:#结果在目标范围内
            aim_times+=1    #瞄准变量自加
        else:   #没有在目标范围内
            aim_times=0 #瞄准变量归零
        if aim_times>50:    #如果瞄准累计到50以上
            mode=1  #进入模式1
            aim_times=0 #瞄准变量归零
#模式1，追球、抓球、检查球
    elif mode==1:#按通篇的逻辑，这部分程序本该在这里写，但最初设计用子函数写了。后续程序复杂就懒得调整了。
        ball_catch(balls_location[max_ball_index][0],balls_location[max_ball_index][1])
#模式2，持球找框
    elif mode==2:
        led.turn_off()  #先关闭补光灯。因为现场亮度足够，而开灯容易识别出蓝色。
#步骤0，转身面向墙壁
        if step==0:
            aim_times=0 #瞄准归零
            drive_close.lock_degrees(True)  #闭环角度锁定打开
            drive_open.move(0,0,0)  #开环移动停止
            drive_close.turn_to_degrees(180,180,waiting_done=False)   #闭环移动转身180度，速度180度/秒
            step_timer_target=1100   #下一步之前延迟1100ms
            step_timer_count=pyb.millis()   #重设计时
            step=1  #进入步骤1
#步骤1，前进找到AprilTags码
        elif step==1:
            if pyb.millis()-step_timer_count>step_timer_target: #达到计时
                drive_close.lock_degrees(False) #闭环角度锁定关闭
                drive_open.move(0,20,0) #开环向前移动
                if tags[0]!=-1: #如果找到Tags码
                    if tags_max.id()<20:    #如果是墙壁的定位码
                        if tags_max.z_translation()*-1<18:  #如果和Tag距离小于18厘米
                            step=2  #进入步骤2
#步骤2，对准AprilTags码
        elif step==2:
            if tags[0]!=-1: #如果找到Tags码
                #计算前进速度的PID值
                front_speed=pid_tags_fb.get_pid((tags_max.z_translation()+12)*-1,1)
                if front_speed>50:  #将前进最大速度限制到50
                    front_speed=50
                #通过PID值控制开环移动库，对准最近的Tags码
                drive_open.move(pid_tags_lr.get_pid(tags_max.cx()-c_x,1),front_speed,\
                pid_tags_r.get_pid((math.pi-tags_max.y_rotation())*-1,1))
            else:   #没找到Tags码
                pid_tags_lr.reset_I()   #重置I
                pid_tags_fb.reset_I()
                pid_tags_r.reset_I()
                drive_open.move(0,0,0)  #停止移动
            if 11<tags_max.z_translation()*-1<13:   #如果对准Tags码在范围内
                aim_times+=1    #瞄准自加
            else:   #没有对准
                aim_times=0 #瞄准归零
            if aim_times>10:    #瞄准大于10
                pid_tags_lr.reset_I()   #重置I
                pid_tags_fb.reset_I()
                pid_tags_r.reset_I()
                drive_open.move(0,0,0)  #停止移动
                step=3 #进入步骤3
                aim_times=0 #瞄准归零
#步骤3，根据当前AprilTags，读取行走线路
        elif step==3:
            side=math.floor(tags_max.id()/5)#判断当前面向那边。以左边为0，顺时针累加。
            #通过当前在哪边、球的颜色，从列表中读取线路
            route=route_to_basket[side][max_ball_index]
            step=4  #进入步骤4
#步骤4，沿着边框，根据AprilTags辅助定位，移动到筐边
        elif step==4:
            if route>0: #行走线路值大于0，则需要向右走
                #向右看到横排最后一个AprilTags码，可能是4/9/14/19
                if tags_max.id()%5==4:
                    if first_time_to_corner:#首次进入
                        route-=1    #线路值自减
                        step_timer_target=1500 #预留1.5秒时间校准车身
                        step_timer_count=pyb.millis()   #重置计时
                    first_time_to_corner=False  #首次进入置否
                    if route==0:    #如果线路值为0了，则到达框了。
                        pid_tags_lr.reset_I()   #重置I
                        pid_tags_fb.reset_I()
                        pid_tags_r.reset_I()
                        drive_open.move(0,0,30) #顺时针自转，为了找筐
                        aim_times=0 #重置瞄准
                        step=5  #进入步骤5
                        arm_control(5)  #机械臂准备放球状态
                    else:#没到达筐
                        if pyb.millis()-step_timer_count<step_timer_target:#延迟时间内
                            #在边缘用Tags校准自身角度
                            drive_open.move(pid_tags_lr.get_pid(tags_max.cx()-c_x,1),\
                                pid_tags_fb.get_pid((tags_max.z_translation()+12)*-1,1),\
                                pid_tags_r.get_pid((math.pi-tags_max.y_rotation())*-1,1))
                        else:   #延迟时间外
                            drive_open.move(0,0,30) #顺时针自转，为了在角落转直角弯，会在自己看到下一个Tags时停止
                elif tags_max.id()<20:#看到 除了4/9/14/19 之外的其他墙壁定位Tags
                    #在没有Tags时，id会保持最后一个检测到的Tags的id。它不是是否存在Tags的判断标准。
                    first_time_to_corner=True   #重置首次进入
                    if tags[0]!=-1: #如果有Tags
                        #以30速度平移的同时，用沿途看到的Tags校准自己的前后、自转量
                        drive_open.move(30,pid_tags_fb.get_pid((tags_max.z_translation()+12)*-1,1),\
                            pid_tags_r.get_pid((math.pi-tags_max.y_rotation())*-1,1))#向右平移
                    else:   #没看到Tags，仅平移。避免持续之前的移动走歪。
                        drive_open.move(30,0,0)
            elif route<0:   #行走线路值小于0，则需要向左走
                #向左看到横排最后一个AprilTags码:0/5/10/15
                if tags_max.id()%5==0:
                    if first_time_to_corner:#首次进入
                        route+=1    #线路值自加
                        step_timer_target=1500 #预留1.5秒时间校准车身
                        step_timer_count=pyb.millis()   #重置计时
                    first_time_to_corner=False  #首次进入置否
                    if route==0:#到达筐
                        pid_tags_lr.reset_I()   #重置I
                        pid_tags_fb.reset_I()
                        pid_tags_r.reset_I()
                        drive_open.move(0,0,-30)    #逆时针自转，为了找筐
                        aim_times=0 #瞄准重置
                        step=5  #进入步骤5
                        arm_control(5)  #机械臂准备放球状态

                    else:#没到达框
                        if pyb.millis()-step_timer_count<step_timer_target:#延迟时间内
                            #在边缘用Tags校准自身角度
                            drive_open.move(pid_tags_lr.get_pid(tags_max.cx()-c_x,1),\
                                pid_tags_fb.get_pid((tags_max.z_translation()+12)*-1,1),\
                                pid_tags_r.get_pid((math.pi-tags_max.y_rotation())*-1,1))
                        else:   #超时
                            drive_open.move(0,0,-30)    #逆时针自转，为了在角落转直角弯会在自己看到下一个Tags时停止
                elif tags_max.id()<20:#看到其他墙壁定位码
                    first_time_to_corner=True   #首次进入角落置真
                    if tags[0]!=-1: #如果有Tags
                        #平移的同时用Tags校准前后和自转量
                        drive_open.move(-30,pid_tags_fb.get_pid((tags_max.z_translation()+12)*-1,1),\
                                pid_tags_r.get_pid((math.pi-tags_max.y_rotation())*-1,1))
                    else:   #没有Tags只平移，避免执行上一条的某个方向移动而走歪
                        drive_open.move(-30,0,0)
#步骤5，用筐前AprilTags定位对准
        elif step==5:
            if tags_max.id()==max_ball_index+20:    #如果当前筐的Tags和目标筐的Tags一致
                #通过给开环控制库装填PID参数，对准Tags
                drive_open.move(pid_tags_lr.get_pid(tags_max.cx()-c_x,1),\
                                pid_tags_fb.get_pid((tags_max.z_translation()+12)*-1,1),\
                                pid_tags_r.get_pid((math.pi-tags_max.y_rotation())*-1,1))
            if 11<tags_max.z_translation()*-1<13:#符合目标值
                aim_times+=1    #瞄准自加
            else:   #不符合目标值
                aim_times=0 #瞄准归零
            if aim_times>20:    #瞄准大于20
                step=6  #进入步骤6
#步骤6，向前移动
        elif step==6:
            drive_open.move(0,20,0) #向前移动
            step_timer_target=500 #预留0.5秒
            step_timer_count=pyb.millis()   #计时重置
            step=7
#步骤7，放球
        elif step==7:
            if pyb.millis()-step_timer_count>step_timer_target: #如果达到时间
                drive_open.move(0,0,0)  #停止移动
                arm_control(6)  #放球
                step_timer_target=500 #预留0.5秒
                step_timer_count=pyb.millis()   #计时重置
                step=8  #进入步骤8
#步骤8，后退
        elif step==8:
            if pyb.millis()-step_timer_count>step_timer_target:#如果达到时间
                drive_open.move(0,-20,0)    #后退
                step_timer_target=500 #预留.5秒
                step_timer_count=pyb.millis()   #计时重置
                step=9  #进入步骤9
#步骤9，顺时针转，找最近的墙壁AprilTags
        elif step==9:
            if pyb.millis()-step_timer_count>step_timer_target:#如果达到时间
                arm_control(0)  #机械臂归位
                drive_open.move(0,0,20) #顺时针自转
                if tags_max.id()%5==0 and tags_max.id()<20: #如果看到筐右侧的Tags的id
                    step_timer_target=2000 #预留2秒为下一步校准自身位置
                    step_timer_count=pyb.millis()   #计时重置
                    step=10 #进入步骤10
#步骤10，用AprilTags校准位置
        elif step==10:
            if pyb.millis()-step_timer_count<step_timer_target:#在延迟时间内
                #通过Tags校准自身位置
                drive_open.move(pid_tags_lr.get_pid(tags_max.cx()-c_x,1),\
                    pid_tags_fb.get_pid((tags_max.z_translation()+12)*-1,1),\
                    pid_tags_r.get_pid((math.pi-tags_max.y_rotation())*-1,1))
            else:#超过延迟时间
                drive_open.move(0,0,0)  #停止移动
                drive_close.lock_degrees(True)  #闭环角度锁定打开
                drive_close.turn_to_degrees(180,180,waiting_done=False) #闭环移动转身
                step_timer_target=1200 #预留1.2秒，给转身执行时间
                step_timer_count=pyb.millis()   #重置计时
                step=11 #进入步骤11
#步骤11，完成一轮步骤
        elif step==11:
            if pyb.millis()-step_timer_count>step_timer_target:#超时执行
                drive_close.lock_degrees(False)  #闭环角度锁定,关闭
                get_balls[max_ball_index]+=1    #给相应的球计数变量自加
                draw_balls()    #重新绘制球显示
                if get_balls==[2,2,2]:  #如果球计数满
                    mode=3  #进入模式3
                    step=0  #步骤归零
                    drive_open.move(0,0,30) #执行庆祝动作：自转
                    servo.degree(1,55)  #执行庆祝动作：抬臂
                    servo.degree(2,125)
                    servo.degree(0,18)
                    step_timer_target=300 #预留0.3秒
                    step_timer_count=pyb.millis()   #计时重置
                else:
                    aim_times=0 #瞄准归零
                    step=0  #步骤归零
                    mode=0 #模式归零
#模式3，完成任务后庆祝姿势
    elif mode==3:
        if step==0:
            if pyb.millis()-step_timer_count>step_timer_target:#达到延迟时间
                servo.degree(0,43)  #开爪子角度
                step_timer_target=300 #0.3秒
                step_timer_count=pyb.millis()   #计时重置
                step=1  #进入步骤1
        if step==1:
            if pyb.millis()-step_timer_count>step_timer_target:#达到延迟时间
                servo.degree(0,16)  #合爪子角度
                step_timer_target=300 #0.3秒
                step_timer_count=pyb.millis()   #计时重置
                step=0  #进入步骤0

#获取电机里程
#    for n in range(4):
#        motor_mileage[n]=drive_close.get_motor_mileage(n)
#本任务应该从轮子运动逆解移动距离，下次完成，下次一定

    #wifi 图传以大于50ms（小于20Hz）的频率运行
    if pyb.millis()-timer_wifi_stream>50:
        timer_wifi_stream=pyb.millis()  #计时重置
        wifi_img.draw_image(img,0,40)   #将摄像头画面绘制到WiFi画布
        wifi_img.draw_rectangle(0,0,160,40,color=(255,255,255))   #显示数据区边框边框
        wifi_img.draw_rectangle(1,1,120,38,fill=True,color=(30,30,30)) #画布上侧绘制显示数据区底色，也用于覆盖刷新已有内容
        wifi_img.draw_rectangle(40,0,40,40,color=(255,255,255))   #显示区域分格线
        wifi_img.draw_line(120,0,120,39,color=(255,255,255))
        #显示时间
        wifi_img.draw_string(83,5,str(math.floor((pyb.millis()-timer)/60000))+':'+str(math.floor(((pyb.millis()-timer)/1000%60))),color=(255,255,255))
        #显示帧速
        if fps<100:#如果显示内容位2位数
            x_offset=8  #x轴坐标补偿8
        else:   #如果显示内容为3位数（没有其他情况）
            x_offset=0  #x轴坐标补偿0
        #绘制帧速数字
        wifi_img.draw_string(78+x_offset,18,str(round(fps)),x_spacing=-4,scale=2,color=(0,255,0))
        #显示小地图
        wifi_img.draw_circle(round(self_x/3),40-round(self_y/3),2,fill=True,color=(255,0,0))
        #显示方向箭头
        wifi_img.draw_arrow(60,20,\
        60+round(math.sin(self_direction)*12),20-round(math.cos(self_direction)*12),thickness=2,color=(255,0,0)) #擦除底色
        #将WiFi画布显示到WiFi图传。这是UP自制的外设和库。
        wifi.display(wifi_img,quality=80)
    fps=clock.fps() #获取帧速。这个函数要在循环尾部，配合循环头部的函数，统计整个循环所用时间。

